// Festival speech synthesis node
//   by David Martin <dave.robert.martin@gmail.com> 2011

#include "ros/ros.h"
#include "std_msgs/String.h"

#include "festival.h"
#include "festival/config.h"
#include "festival/snprintf.h"
#include "festival/utils.h"

#include "festival/Speech.h"
#include "festival/Voice.h"
#include "festival/GetVoiceSet.h"
#include "festival/SetVoice.h"

#include "dialog_manager/ToggleEars.h"

std::vector<festival::Voice> voiceSet;
ros::ServiceClient cliToggleEars;
dialog_manager::ToggleEars srvToggleEars;

bool getVoiceSet(
        festival::GetVoiceSet::Request  &req,
        festival::GetVoiceSet::Response &res ) {
  if (!updateFestivalVoiceSet(&voiceSet)) {
    ROS_ERROR("Unable to determine available voices");
    return false;
  }
  
  res.voices = voiceSet;
  
  return true;
}

bool setVoice(
        festival::SetVoice::Request  &req,
        festival::SetVoice::Response &res ) {
  res.result = false;
  
  //
  // Change this to work with dynamic reconfigure?
  //
  
  // Check that the voice requested is in the list of known voices
  std::vector<festival::Voice>::iterator it;
  for (it = voiceSet.begin(); it != voiceSet.end(); ++it)
    if (it->name == req.voice.name) {
      break;
  }
  
  if (it == voiceSet.end()) {
    ROS_ERROR("Unsupported voice: %s", req.voice.name.c_str());
    return true;
  }
  
  // Attempt to set the voice
  char *cmdSetVoice;
  if (asprintf(&cmdSetVoice, "(voice_%s)", req.voice.name.c_str()) >= 0) {
    res.result = festival_eval_command(cmdSetVoice);

    // If the voice was set, reset vars  
    if (res.result) {
      ROS_INFO("Voice changed to %s", req.voice.name.c_str());
      
      if (!festival_eval_command("(voice_reset)"))
        ROS_WARN("Unabled to reset vars after setting voice to %s", req.voice.name.c_str());
    }
  }
  
  return true;
}

void speechCallback(const festival::Speech::ConstPtr& msg) {
  ROS_DEBUG("About to speak");
  srvToggleEars.request.state = false;
  cliToggleEars.call(srvToggleEars);
  if(bool(festival_say_text(msg->text.c_str()))) {
    srvToggleEars.request.state = true;
    cliToggleEars.call(srvToggleEars);
    ROS_INFO("Festival said: %s", msg->text.c_str());
  } else
    ROS_ERROR("Festival was unable to speak!");
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "festival_speaker");
  ros::NodeHandle n;

  // Initalize festival
  festival_initialize(1, i_HEAP_SIZE);
  
  // Determine available voices
  if (!updateFestivalVoiceSet(&voiceSet)) {
    ROS_ERROR("Unable to determine available voices.  Exiting...");
    return 1;
  }

#ifdef ASYNC_festival
  festival_eval_command("(audio_mode 'async)");
#endif

  // Use an external playback method
  //festival_eval_command("(Parameter.set 'Audio_Required_Format 'aiff)");
  //festival_eval_command("(Parameter.set 'Audio_Command \"play --type aiff $FILE\")");
  //festival_eval_command("(Parameter.set 'Audio_Method 'Audio_Command)");
  
  // Advertise services
  ros::ServiceServer srvGetVoiceSet = n.advertiseService("get_voice_set", getVoiceSet);
  ros::ServiceServer srvSetVoice    = n.advertiseService("set_voice",     setVoice);
  
  // Subscribe to topics
  ros::Subscriber sub = n.subscribe("speech", 100, speechCallback);
  
  // Activate Clients
  cliToggleEars = n.serviceClient<dialog_manager::ToggleEars>("toggle_ears");
  
  ROS_INFO("Ready to speak!");
  ros::spin();
  
  return 0;
}

